/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-11 10:09:50
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-07-18 16:32:53
 * @FilePath: \zino-fc-v4\ZINO\sensor\sensordata.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-09-11     UAVI02       the first version
 */
#ifndef __FC_SENSORDATA_H__
#define __FC_SENSORDATA_H__

#ifdef __cplusplus
extern "C"
{
#endif

#include <rtthread.h>
#include <stdbool.h>
#include "maths.h"
#include "zino_config.h"

#define ACC_GYRO_CALIBRATE_SAMPLE 1200
#define BARO_CALIBRATE_SAMPLE 600
#define OPT_RESOLUTION (0.2131946f)        /*1m高度下 1个像素对应的位移，单位cm*/
#define OPT_SPEED_RESOLUTION (0.02131946f) /*y = 7.7554x, y: speed(m/s), x: height(m)*/
#define USELPF
    typedef struct
    {
        float pressure;
        float temperature;
        float altitude;
        float offset;
    } baro_t;

    typedef struct
    {
        bool isdatacalibrated;
        uint16_t samplesize;
        Axis_3_int16_t zero;
        Axis_3_int32_t samplesum;
    } calibrate_t;

    typedef struct sensordata_acc
    {
        Axis_3_int16_t accRaw; // raw adc
        Axis_3_float_t accdata;
        Axis_3_float_t acclpf;  // g/ss
        Axis_3_float_t accSum;  // intergral
        calibrate_t acc_offset; // mg/ss
        Axis_3_int32_t sumCount;
        float scacle;

    } acc_t;
    typedef struct sensordata_gyro
    {
        Axis_3_int16_t gyroRaw;  // raw adc
        Axis_3_float_t gyrodata; // deg/s
        Axis_3_float_t gyrolpf;  // gdeg/s
        calibrate_t gyro_offset; // mdeg/s
        Axis_3_float_t gyroSum;  // intergral
        Axis_3_int32_t sumCount;
        float scacle;
    } gyro_t;
    // 激光数据，原始数据（无需处理和融合）
    typedef struct time_of_flight
    {
        uint16_t range;   // mm
        uint16_t quality; // 0-100
        bool isOnline;
    } tof_t;
    // 光流传感器数据（光流与角速度融合后的）
    typedef struct
    {
        int16_t optVelX;
        int16_t optVelY;
        int16_t optPosX;
        int16_t optPosY;
        int16_t optQual;
        uint16_t shutter;
        uint8_t is_valid;
        float lpfX;
        float lpfY;
        float fixVelX;
        float fixVelY;
        float fixPosX;
        float fixPosY;
        bool isOpFlowOk;
    } optf_t;

    typedef struct
    {
        float optSum[2];
        float optfix[2];
        float optValid[2];
        float optValidLast[2];

        float deltaPos[2];
        float deltaVel[2];
        float pos[2];
        float vel[2];
    } opt_fix_t;

    typedef union
    {
        struct
        {
            float roll;
            float pitch;
            float yaw;
        };
        float axis[3]; // ROLL PITCH YAW
    } attitude_t;
    // 处理后的传感器数据（校准、融合等）
    typedef struct fc_sensordata
    {
        acc_t acc;
        gyro_t gyro;
        baro_t baro;
        tof_t tof;
        optf_t optf_raw;
        opt_fix_t optf;
        attitude_t attitude;
    } fc_sensor_data_t;

    // 当前状态数据，来自融合数据(fc_sensor_data_t)
    typedef struct
    {
        Axis_3_float_t acc;      // 加速度
        Axis_3_float_t vel;      // 速度
        Axis_3_float_t pos;      // 位置
        attitude_t attitude;     // 角度
        attitude_t attitudeRate; // 角速度
    } zdrone_state_t;

    typedef struct ZeroOffset
    {
        Axis_3_int16_t accoffset;
        Axis_3_int16_t gyrooffset;
        uint16_t acc_cutoff;
        uint16_t gyro_cutoff;
        uint8_t flag;
        bool acc_calibrated;
        bool gyro_calibrated;
        bool factorycalibrated;
    } imu_offset_t;

    struct optical_flow
    {
        uint8_t is_valid;      // 0:invalid, 1:valid
        uint8_t is_withHeight; // 0:invalid, 1:valid
        uint8_t is_withIMU;    // 0:invalid, 1:valid
        float raw_x;          // origin vel x
        float raw_y;          // origin vel y
        float hfix_x;       // with height fuse
        float hfix_y;       // with height fuse
        float gfix_x;        // with gyro fuse
        float gfix_y;        // with gyro fuse
        float afix_x;         // with acc fuse
        float afix_y;         // with acc fuse
        float pos_x;         // with pos fuse
        float pos_y;         // with pos fuse
        uint16_t quality; // 0-100
        uint16_t shutter;
    };

#define IMU_SAVED_FLAG 0X28
#define ACC_LPF_CUTOFF 400
#define GYRO_LPF_CUTOFF 400
#define THRUST_BASE 400.0f
#define MAX_TILT_ANGLE 90
#define MAX_RC_ANGLE 30
#define MAX_RC_RASING 100

    // 观测数据，来自融合后的数据
    ZINO_CONFIG_DECLARE(imu_offset_t, imu_offset);
    void imu_reset_config();
    extern fc_sensor_data_t fc_sensor_data;
    extern zdrone_state_t zdrone_state;
    void sensor_acc_init(uint16_t cutoff);
    void sensor_gyro_init(uint16_t cutoff);
    void acc_update(void);
    void gyro_update(void);
    void tof_update(void);
    void AccStartCalibration(void);
    void GyroStartCalibration(void);
    void sensor_acc_process(acc_t *acc);
    void sensor_gyro_process(gyro_t *acc);
    void sensor_baro_updata(void);

    void opticalflow_sensor_handle_init(void);
    void opticalflow_sensor_handle(void);
    // void sensor_gyro_process(struct rt_sensor_data *sensor);
    // void sensor_baro_handle(struct rt_sensor_data *baro_sensor, struct rt_sensor_data *temp_sensor);
    // void sensor_tof_handle(struct rt_sensor_data *sensor);
    // void sensor_opticlFlow_handle(struct rt_sensor_data *sensor);
    // void sensor_opticlFlow_handle(struct rt_sensor_data *sensor);

    // void acc_trace(fc_sensor_data_t *sensor, zdrone_state_t *state, float dt);

    extern bool baro_online;
    extern bool acc_gyro_online;
    extern bool laser_online;
    extern bool opticalflow_online;
    extern struct optical_flow fc_optical;
#ifdef __cplusplus
}
#endif

#endif /* ZINO_SENSOR_SENSOR_H_ */
